/*
 * @Author: fuhongliang-TSJ hongliang.fu@4d-bios.com
 * @Date: 2025-08-19 16:07:38
 * @LastEditTime: 2025-08-19 17:26:28
 * @LastEditors: fuhongliang-TSJ hongliang.fu@4d-bios.com
 * @Description: 
 * @FilePath: \lc_-esp32_-s3\components\bsp\qmi8658\qmi8658.c
 * 立创实战派ESP32-S3开发板
 * Copyright (c) 2025 by 4D Bios, All Rights Reserved. 
 */
#include "qmi8658.h"
#include "esp_log.h"
#include <math.h>
#include <stdint.h>

static const char* TAG = "QMI8658";

i2c_obj_t qmi8658_master;

/**
 * @brief       读取ap3216c的16位IO值
 * @param       data:存储区
 * @param       reg :寄存器
 * @retval      ESP_OK:读取成功；其他:读取失败
 */
static esp_err_t qmi8658_read_one_byte(uint8_t* data, uint8_t reg)
{
    uint8_t reg_buf[1];

    reg_buf[0] = reg;

    i2c_buf_t buf[2] = {
        {.len = 1, .buf = reg_buf},
        {.len = 1, .buf = data},
    };

    i2c_transfer(&qmi8658_master, QMI8658_ADDR, 2, buf, I2C_FLAG_WRITE | I2C_FLAG_READ | I2C_FLAG_STOP);
    return ESP_OK;
}

/**
 * @brief       向ap3216c写入16位IO值
 * @param       data:要写入的数据
 * @retval      ESP_OK:读取成功；其他:读取失败
 */
static esp_err_t qmi8658_write_one_byte(uint8_t reg, uint8_t data)
{
    uint8_t reg_buf[1];

    reg_buf[0] = reg;
    
    i2c_buf_t buf[2] = {
        {.len = 1, .buf = reg_buf},
        {.len = 1, .buf = &data},
    };

    i2c_transfer(&qmi8658_master, QMI8658_ADDR, 2, buf, I2C_FLAG_STOP); /* 传输传感器地址 */
    return ESP_OK;
}

/**
 * @description: 读取寄存器地址数据
 * @param {uint8_t} reg_addr
 * @param {uint8_t} *data
 * @param {size_t} len
 * @return {*}
 */
esp_err_t qmi8658_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_write_read_device(qmi8658_master.port, QMI8658_ADDR,  &reg_addr, 1, data, len, 1000 / portTICK_PERIOD_MS);
}

/**
 * @description: 写入寄存器地址数据
 * @param {uint8_t} reg_addr
 * @param {uint8_t} data
 * @return {*}
 */
esp_err_t qmi8658_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    uint8_t write_buf[2] = {reg_addr, data};

    return i2c_master_write_to_device(qmi8658_master.port, QMI8658_ADDR, write_buf, sizeof(write_buf), 1000 / portTICK_PERIOD_MS);
}

/**
 * @description: 读取加速度和陀螺仪寄存器值
 * @param {qmi8658_t} *p
 * @return {*}
 */
void qmi8658_Read_AccAndGry(qmi8658_t *p)
{
    uint8_t status, data_ready=0;
    int16_t buf[6];

    qmi8658_register_read(QMI8658_STATUS0, &status, 1); // 读状态寄存器
    if (status & 0x03) // 判断加速度和陀螺仪数据是否可读
        data_ready = 1;
    if (data_ready == 1){  // 如果数据可读
        data_ready = 0;
        qmi8658_register_read(QMI8658_AX_L, (uint8_t *)buf, 12); // 读加速度和陀螺仪值
        p->acc_x = buf[0];
        p->acc_y = buf[1];
        p->acc_z = buf[2];
        p->gyr_x = buf[3];
        p->gyr_y = buf[4];
        p->gyr_z = buf[5];
    }
}

/**
 * @description: 获取XYZ轴的倾角值
 * @param {qmi8658_t} *p
 * @return {*}
 */
void qmi8658_fetch_angleFromAcc(qmi8658_t *p)
{
    float temp;

    qmi8658_Read_AccAndGry(p); // 读取加速度和陀螺仪的寄存器值
    // 根据寄存器值 计算倾角值 并把弧度转换成角度
    temp = (float)p->acc_x / sqrt( ((float)p->acc_y * (float)p->acc_y + (float)p->acc_z * (float)p->acc_z) );
    p->AngleX = atan(temp)*57.29578f; // 180/π=57.29578
    temp = (float)p->acc_y / sqrt( ((float)p->acc_x * (float)p->acc_x + (float)p->acc_z * (float)p->acc_z) );
    p->AngleY = atan(temp)*57.29578f; // 180/π=57.29578
    temp = sqrt( ((float)p->acc_x * (float)p->acc_x + (float)p->acc_y * (float)p->acc_y) ) / (float)p->acc_z;
    p->AngleZ = atan(temp)*57.29578f; // 180/π=57.29578
}

/**
 * @brief       配置qmi8658
 * @param       无
 * @retval      0, 成功;
                1, 失败;
*/
uint8_t qmi8658_comfig(void)
{
    uint8_t id = 0;
    
    qmi8658_register_read(QMI8658_WHO_AM_I, &id, 1);

    qmi8658_register_write_byte(QMI8658_RESET, 0xb0);  // 复位
    vTaskDelay(10 / portTICK_PERIOD_MS);
    // 配置运动状态检测
    qmi8658_register_write_byte(QMI8658_CATL1_L, 1); // AnyMotionXThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL1_H, 1); // AnyMotionYThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL2_L, 1); // AnyMotionZThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL2_H, 1); // NoMotionXThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL3_L, 1); // NoMotionYThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL3_H, 1); // NoMotionZThr 必须是0~32之间的数
    qmi8658_register_write_byte(QMI8658_CATL4_L, 0x77); // MOTION_MODE_CTRL 0111 0111
    qmi8658_register_write_byte(QMI8658_CATL4_H, 0x01); // 0x01(means 1st command)
    qmi8658_register_write_byte(QMI8658_CTRL9, 0x0E); // CTRL_CMD_CONFIGURE_MOTION

    qmi8658_register_write_byte(QMI8658_CATL1_L, 1); // AnyMotionWindow 
    qmi8658_register_write_byte(QMI8658_CATL1_H, 1); // NoMotionWindow 
    qmi8658_register_write_byte(QMI8658_CATL2_L, 0xE8); // SigMotionWaitWindow[7:0]
    qmi8658_register_write_byte(QMI8658_CATL2_H, 0x03); // SigMotionWaitWindow [15:8]
    qmi8658_register_write_byte(QMI8658_CATL3_L, 0xE8); // SigMotionConfirmWindow[7:0]
    qmi8658_register_write_byte(QMI8658_CATL3_H, 0x03); // SigMotionConfirmWindow[15:8]
    // qmi8658_register_write_byte(QMI8658_CATL4_L, 0x40); // NA
    qmi8658_register_write_byte(QMI8658_CATL4_H, 0x02); // 0x02(means 2nd command)
    qmi8658_register_write_byte(QMI8658_CTRL9, 0x0E); // CTRL_CMD_CONFIGURE_MOTION

    qmi8658_register_write_byte(QMI8658_CTRL1, 0x40); // CTRL1 设置地址自动增加
    qmi8658_register_write_byte(QMI8658_CTRL7, 0x03); // CTRL7 允许加速度和陀螺仪
    qmi8658_register_write_byte(QMI8658_CTRL2, 0x95); // CTRL2 设置ACC 4g 250Hz
    qmi8658_register_write_byte(QMI8658_CTRL3, 0xd5); // CTRL3 设置GRY 512dps 250Hz

    qmi8658_register_write_byte(QMI8658_CTRL8, 0x0E); // CTRL7 允许Any-Motion No-Motion and Significant-Motion
 
    if (id == 0X05)
    {
        ESP_LOGI(TAG, "QMI8658 Success!!!");
        return 0;                       /* AP3216C正常 */
    }
    else
    {
        ESP_LOGE(TAG, "QMI8658 Fail!!!");
        return 1;                       /* AP3216C失败 */
    }
} 

// 关闭芯片运行
void qmi8658_close(void)
{
    qmi8658_register_write_byte(QMI8658_CTRL1, 0x01); // 关闭芯片运行
}

// 获取Motion状态
uint8_t qmi8658_fetch_motion(void)
{
    uint8_t status = 0;
    qmi8658_register_read(QMI8658_STATUS1, &status, 1); // 读状态寄存器 
    return status;
}

/**
 * @brief       初始化qmi8658
 * @param       无
 * @retval      无
 */
void qmi8658_init(i2c_obj_t self)
{
    if (self.init_flag == ESP_FAIL)
    {
        iic_init(I2C_NUM_0);    /* 初始化IIC */
    }

    qmi8658_master = self;

    while (qmi8658_comfig())    /* 检测不到AP3216C */
    {
        ESP_LOGE(TAG, "QMI8658 init Fail!!!");
        vTaskDelay(500);
    }
}